

#include <iostream>

extern "C"{
#include <gst/gst.h>
}
#include <glib.h>
#include <ros/ros.h>
#include "gstcamera/gstcam_pc.h"
#include "gstcamera/video_status.h"


namespace gstcamera
{
    //构造函数
    Gstcam_pc::Gstcam_pc(ros::NodeHandle nh_camera,ros::NodeHandle nh_private):
    //client_ip_("127.0.0.1"),
    //client_port_(554),
    nh_(nh_camera),
    nh_private_(nh_private)
    {
     video_server = nh_.advertiseService("video_switch_srv",&Gstcam_pc::video_switch,this);
    }

    Gstcam_pc::~Gstcam_pc()
    {
        ROS_INFO_STREAM("end of the ROS gstreamer stream");
    }

/*
##################################################################
## Initialize and  configure the udp/RTP (直接复制pipeline)streaming pipeline
###################################*************************
*/
bool Gstcam_pc::configure_stream()
  {

    //get additional gstcamera configur

    // nh_private_.getParam("client_ip",client_ip_);
    // nh_private_.getParam("client_port",client_port_);

    // nh_private_.param("width",width_,384);
    // nh_private_.param("height",height_,288);
    // nh_private_.param("framerate",framerate_,25);

    bool gst_config_status;
    gst_config_status = nh_private_.getParam("gstconfig",gstconfig_);
    if(gst_config_status == false){
        ROS_FATAL("can not get the gstconfig_launch pipeline");
    }
     ROS_INFO_STREAM("Using gstreamer config from rosparam:"<<"\r\n"<<gstconfig_<<"\n");

    if(! gst_is_initialized()){
      // Initialize gstreamer pipeline
      ROS_INFO_STREAM( "Initializing gstreamer..." );
      gst_init(0,0);  
    }
    ROS_INFO_STREAM( "Gstreamer Version: " << gst_version_string() );

    GError *error = 0; // Assignment to zero is a gst requirement
    //从参数管理器中加载gstreamer pipeline
    pipeline_ = gst_parse_launch(gstconfig_.c_str(), &error);  
    //如果失败了  
    if (pipeline_ == NULL) {
      ROS_FATAL_STREAM( error->message );
      return false;
    }
   
    //准备启动pipeline
    gst_element_set_state(pipeline_, GST_STATE_PAUSED);
    if (gst_element_get_state(pipeline_, NULL, NULL, -1) == GST_STATE_CHANGE_FAILURE) {
      ROS_FATAL("Failed to PAUSE stream, check your gstreamer configuration.");
      return false;
    } 
    else {
      ROS_INFO_STREAM("Stream is PAUSED.");
    }

    return true;
  } //end of Gstcam_pc::configure_stream Function


/*
###################################
## configure TX1　STREAM
###################################
*/
bool Gstcam_pc::configure_TX1_strema()
{
  GstElement  *src, *src_filter,*encode, *enc_filter,*rtppack, *sink;
  GstCaps *source_caps,*encode_caps;
  GstBus *bus;
  GstMessage *msg;
  GstStateChangeReturn ret;
//从　launch中读取参数，这里的是初值
// std::string   device_param =  "/dev/video0";
   std::string   device_param =  "";
   //std::string   gst_config_param = "video/x-raw, format=(string)I420, width=640, heigth=480, framerate=30/1";
   std::string   gst_config_param = "";
  // std::string   udp_url_param = "host=192.168.0.101, port=5000, sync=FALSE, async=FALSE";
   std::string   ip_param = "";
   int  port_param;

  nh_private_.getParam("DEVICE",device_param);
  nh_private_.getParam("gst_config",gst_config_param);
  nh_private_.getParam("host",ip_param);
  nh_private_.getParam("port",port_param);
//  nh_private.get_Param("udp_url",udp_url_param);

/* Initialize GStreamer */
  gst_init (0,0);

  /* Create the elements */
  src	 = gst_element_factory_make("v4l2src", "src");
  src_filter = gst_element_factory_make("capsfilter", "src_filter");
  encode = gst_element_factory_make("omxh264enc", "encode");
  enc_filter = gst_element_factory_make("capsfilter", "enc_filter");
  rtppack = gst_element_factory_make("rtph264pay", "rtppack");
  sink = gst_element_factory_make("udpsink", "sink");

  /* Create the empty pipeline */
  pipeline_ = gst_pipeline_new("TX1_pipeline");

  if (!pipeline_ || !src || !src_filter ||  !encode  || !rtppack || !sink) {
    ROS_ERROR ("\r\n Not all elements could be created.\n");
    return false;
  }

  source_caps  = gst_caps_from_string(gst_config_param.c_str());
//source_caps  = gst_caps_from_string("video/x-raw, width=640, height=480,framerate=30/1, format=(string)I420");
encode_caps  = gst_caps_from_string("video/x-h264, stream-format=(string)byte-stream");

  
  //sink_caps   = gst_caps_from_string(udp_url_param.c_str());
 
  g_object_set(G_OBJECT(src), "device",device_param.c_str(), NULL); //设备端口号
 // g_object_set(G_OBJECT(src), "device","/dev/video0", NULL); //设备端口号
  g_object_set(G_OBJECT(src_filter),"caps", source_caps, NULL); //输入源的尺寸和格式
  g_object_set(G_OBJECT(enc_filter),"caps", encode_caps, NULL); //输入源的尺寸和格式
   //g_object_set( G_OBJECT(rtp264), "pt", 96, "config-interval",1,NULL);
  g_object_set( G_OBJECT(rtppack), "pt", 96,NULL); //转化为RTP包的一个范围设置
  g_object_set( G_OBJECT(sink), "host", ip_param.c_str(), "port", port_param, "sync",FALSE, "async", FALSE, NULL);

  ROS_INFO("\r\n the pipeline set properties successfully......\n");

 /* Build the pipeline */
  gst_bin_add_many(GST_BIN(pipeline_), src, src_filter, encode, enc_filter,rtppack, sink, NULL);
  if(gst_element_link_many(src,src_filter, encode, enc_filter,rtppack,sink,NULL) != TRUE) {
    ROS_INFO ("\r\n Elements could not be linked.\n");
    gst_object_unref(pipeline_);
    return false;
  }
    ROS_INFO("\r\n the pipeline linked successfully......\n");


  /* Start playing */
  ret = gst_element_set_state(pipeline_, GST_STATE_PLAYING);
  if(ret == GST_STATE_CHANGE_FAILURE) {
    ROS_ERROR("Unable to set the pipeline to the playing state.\n");
    gst_object_unref (pipeline_);
    return false;
  }
    ROS_INFO("running......\n");
    return true; 
 // ROS_INFO_STREAM("the pipelint is \r"<< "v4l2src ! "<< gst_config_param << "! omxh264enc ! "  <<" rtph264pay ! "<<"udpsink" << udp_url_param);
  
    // Parse message 消息的处理 
    /*
  if (msg != NULL) {
    GError *err;
    gchar *debug_info;

    switch (GST_MESSAGE_TYPE (msg)) {
      case GST_MESSAGE_ERROR:
        gst_message_parse_error (msg, &err, &debug_info);
        g_printerr ("Error received from element %s: %s\n", GST_OBJECT_NAME (msg->src), err->message);
        g_printerr ("Debugging information: %s\n", debug_info ? debug_info : "none");
        g_clear_error (&err);
        g_free (debug_info);
        break;
      case GST_MESSAGE_EOS:
        g_print ("End-Of-Stream reached.\n");
        break;
      default:
        // We should not reach here because we only asked for ERRORs and EOS 
        g_printerr ("Unexpected message received.\n");
        break;
    }
    gst_message_unref (msg);
  }
  */

  /* Free resources */
 // gst_object_unref(bus);
 // gst_element_set_state(pipeline_, GST_STATE_NULL);
 // gst_object_unref(pipeline_);
  
} //end of Gstcam_pc::configure_TX1_strema()


/*
###################################
## start stream
###################################
*/
void Gstcam_pc::start_stream()
{
    ROS_INFO("Starting stream........");  
    if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
    ROS_ERROR("Could not start stream!");
        }
    // while (ros::ok())
    // {
    //     ros::spinOnce();
    // }
    ros::spin();
    //ROS_INFO("Started stream.");   
}

/*
###################################
## start stream
###################################
*/
void Gstcam_pc::stop_stream()
{
    ROS_INFO("stop stream........");  
    if(gst_element_set_state(pipeline_, GST_STATE_PAUSED) == GST_STATE_CHANGE_FAILURE) {
    ROS_ERROR("Could not stop stream!");
        }
    // while (ros::ok())
    // {
    //     ros::spinOnce();
    // }
    ros::spin();
    //ROS_INFO("Started stream.");   
}



/*
###################################
## stop the streamer ##
###################################
*/
  void Gstcam_pc::cleanup_stream()
  {
    // Clean up
    ROS_INFO("clearnup gstreamer pipeline...");
   
      gst_element_set_state(pipeline_, GST_STATE_NULL);
      gst_object_unref(pipeline_);
      pipeline_ = NULL;
    
  } //end of the cleanup_stream Function


/*
###################################
## running the streamer ##
###################################
*/
void Gstcam_pc::RTP_run()
    {
        if(!this->configure_stream())
        {
            ROS_WARN("\r configuring  gscam!");
        }
        this->start_stream();

        //clean up 
        this->cleanup_stream();
      ROS_INFO("GStreamer stream stopped!");
    }   //end of the RTP_run Function


/*
###################################
## running the streamer ##
###################################
*/
void Gstcam_pc::TX1_run()
    {
        if(!this->configure_TX1_strema());
        {
            ROS_WARN(" \r configuring  gscam!");
        }
        this->start_stream();

        //clean up 
        this->cleanup_stream();
      ROS_INFO("GStreamer stream stopped!");
    }   //end of the RTP_run Function



/*
###################################
## 回调函数 ##
###################################
*/
//server
bool video_status = true;
 bool Gstcam_pc::video_switch(
    gstcamera::video_status::Request    &request_sta,
    gstcamera::video_status::Response   &Response_sta
)
{
    ROS_INFO(" receive the client request the video status Play/Pause [%d]\r\n",request_sta.status_);
    video_status = !video_status;
    
    if(video_status==true)
    {
    ROS_INFO("Starting stream........");  
    if(gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) {
    ROS_ERROR("Could not start stream!");
        }
    }
    else 
    {
    ROS_INFO("stop stream........");  
    if(gst_element_set_state(pipeline_, GST_STATE_PAUSED) == GST_STATE_CHANGE_FAILURE) {
    ROS_ERROR("Could not stop stream!");
        }
    }

    Response_sta.success_=1;
    return Response_sta.success_;
}


} // end of gstcamera namespace



